// Copyright (c) 2021 by Rockchip Electronics Co., Ltd. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <sys/time.h>
#include <vector>
#include <set>
#include "postprocess.h"
#include <stdint.h>


#include "rk_debug.h"
#include "rk_defines.h"
#define LABEL_NALE_TXT_PATH "/oem/usr/model/yolo_label.txt"

static float RGBYUV02990[256], RGBYUV05870[256], RGBYUV01140[256];
static float RGBYUV01684[256], RGBYUV03316[256];
static float RGBYUV04187[256], RGBYUV00813[256];

static float RGBYUV14075[256], RGBYUV03455[256], RGBYUV07169[256], RGBYUV17790[256];



static char *labels[OBJ_CLASS_NUM];

const int anchor0[6] = {10, 13, 16, 30, 33, 23};
const int anchor1[6] = {30, 61, 62, 45, 59, 119};
const int anchor2[6] = {116, 90, 156, 198, 373, 326};

inline static int clamp(float val, int min, int max)
{
    return val > min ? (val < max ? val : max) : min;
}

char *readLine(FILE *fp, char *buffer, int *len)
{
    int ch;
    int i = 0;
    size_t buff_len = 0;

    buffer = (char *)malloc(buff_len + 1);
    if (!buffer)
        return NULL; // Out of memory

    while ((ch = fgetc(fp)) != '\n' && ch != EOF)
    {
        buff_len++;
        void *tmp = realloc(buffer, buff_len + 1);
        if (tmp == NULL)
        {
            free(buffer);
            return NULL; // Out of memory
        }
        buffer = (char *)tmp;

        buffer[i] = (char)ch;
        i++;
    }
    buffer[i] = '\0';

    *len = buff_len;

    // Detect end
    if (ch == EOF && (i == 0 || ferror(fp)))
    {
        free(buffer);
        return NULL;
    }
    return buffer;
}

int readLines(const char *fileName, char *lines[], int max_line) //读取文件的每行
{
    FILE *file = fopen(fileName, "r");
    char *s;
    int i = 0;
    int n = 0;
    while ((s = readLine(file, s, &n)) != NULL)
    {
        lines[i++] = s;
        if (i >= max_line)
            break;
    }
    return i;
}

int loadLabelName(const char *locationFilename, char *label[]) //读取文件
{
    RK_LOGI("loadLabelName %s\n", locationFilename);
    readLines(locationFilename, label, OBJ_CLASS_NUM);
    return 0;
}

static float CalculateOverlap(float xmin0, float ymin0, float xmax0, float ymax0, float xmin1, float ymin1, float xmax1, float ymax1)
{
    float w = fmax(0.f, fmin(xmax0, xmax1) - fmax(xmin0, xmin1) + 1.0);
    float h = fmax(0.f, fmin(ymax0, ymax1) - fmax(ymin0, ymin1) + 1.0);
    float i = w * h;
    float u = (xmax0 - xmin0 + 1.0) * (ymax0 - ymin0 + 1.0) + (xmax1 - xmin1 + 1.0) * (ymax1 - ymin1 + 1.0) - i;
    return u <= 0.f ? 0.f : (i / u);
}

static int nms(int validCount, std::vector<float> &outputLocations, std::vector<int> classIds, std::vector<int> &order,int filterId, float threshold)
{
    for (int i = 0; i < validCount; ++i)
    {
        if (order[i] == -1|| classIds[i] != filterId)
        {
            continue;
        }
        int n = order[i];
        for (int j = i + 1; j < validCount; ++j)
        {
            int m = order[j];
            if (m == -1 || classIds[i] != filterId)
            {
                continue;
            }
            float xmin0 = outputLocations[n * 4 + 0];
            float ymin0 = outputLocations[n * 4 + 1];
            float xmax0 = outputLocations[n * 4 + 0] + outputLocations[n * 4 + 2];
            float ymax0 = outputLocations[n * 4 + 1] + outputLocations[n * 4 + 3];

            float xmin1 = outputLocations[m * 4 + 0];
            float ymin1 = outputLocations[m * 4 + 1];
            float xmax1 = outputLocations[m * 4 + 0] + outputLocations[m * 4 + 2];
            float ymax1 = outputLocations[m * 4 + 1] + outputLocations[m * 4 + 3];

            float iou = CalculateOverlap(xmin0, ymin0, xmax0, ymax0, xmin1, ymin1, xmax1, ymax1);

            if (iou > threshold)
            {
                order[j] = -1;
            }
        }
    }
    return 0;
}

static int quick_sort_indice_inverse(
    std::vector<float> &input,
    int left,
    int right,
    std::vector<int> &indices)
{
    float key;
    int key_index;
    int low = left;
    int high = right;
    if (left < right)
    {
        key_index = indices[left];
        key = input[left];
        while (low < high)
        {
            while (low < high && input[high] <= key)
            {
                high--;
            }
            input[low] = input[high];
            indices[low] = indices[high];
            while (low < high && input[low] >= key)
            {
                low++;
            }
            input[high] = input[low];
            indices[high] = indices[low];
        }
        input[low] = key;
        indices[low] = key_index;
        quick_sort_indice_inverse(input, left, low - 1, indices);
        quick_sort_indice_inverse(input, low + 1, right, indices);
    }
    return low;
}

static float sigmoid(float x)
{
    return 1.0 / (1.0 + expf(-x));
}

static float unsigmoid(float y)
{
    return -1.0 * logf((1.0 / y) - 1.0);
}

inline static int32_t __clip(float val, float min, float max)
{
    float f = val <= min ? min : (val >= max ? max : val);
    return f;
}

static int8_t qnt_f32_to_affine(float f32, int32_t zp, float scale)
{
    float dst_val = (f32 / scale) + zp;
    int8_t res = (int8_t)__clip(dst_val, -128, 127);
    return res;
}

static float deqnt_affine_to_f32(int8_t qnt, int32_t zp, float scale)
{
    return ((float)qnt - (float)zp) * scale;
}

static int process(int8_t *input, int *anchor, int grid_h, int grid_w, int height, int width, int stride,
                   std::vector<float> &boxes, std::vector<float> &objProbs, std::vector<int> &classId,
                   float threshold, int32_t zp, float scale)
{

    int validCount = 0;
    int grid_len = grid_h * grid_w;
    RK_LOGD("===grid len is %d==\n",grid_len);
    float thres = unsigmoid(threshold);
    int8_t thres_i8 = qnt_f32_to_affine(thres, zp, scale);
    RK_LOGD("====thres is %d, thres_i8 is %d===\n",thres, thres_i8);

    for (int a = 0; a < 3; a++)
    {
        for (int i = 0; i < grid_h; i++)
        {
            for (int j = 0; j < grid_w; j++)
            {
                int8_t box_confidence = input[(PROP_BOX_SIZE * a + 4) * grid_len + i * grid_w + j];
                // RK_LOGI("==box confi is %d===\n",box_confidence);

                if (box_confidence >= thres_i8)
                {
                    int offset = (PROP_BOX_SIZE * a) * grid_len + i * grid_w + j;
                    int8_t *in_ptr = input + offset;
                    // RK_LOGI("===offset is %d, in_ptr is %d===\n", offset, in_ptr);

                    float box_x = sigmoid(deqnt_affine_to_f32(*in_ptr, zp, scale)) * 2.0 - 0.5;
                    float box_y = sigmoid(deqnt_affine_to_f32(in_ptr[grid_len], zp, scale)) * 2.0 - 0.5;
                    float box_w = sigmoid(deqnt_affine_to_f32(in_ptr[2 * grid_len], zp, scale)) * 2.0;
                    float box_h = sigmoid(deqnt_affine_to_f32(in_ptr[3 * grid_len], zp, scale)) * 2.0;
                    box_x = (box_x + j) * (float)stride;
                    box_y = (box_y + i) * (float)stride;
                    box_w = box_w * box_w * (float)anchor[a * 2];
                    box_h = box_h * box_h * (float)anchor[a * 2 + 1];
                    box_x -= (box_w / 2.0);
                    box_y -= (box_h / 2.0);

                    int8_t maxClassProbs = in_ptr[5 * grid_len];
                    int maxClassId = 0;
                    for (int k = 1; k < OBJ_CLASS_NUM; ++k)
                    {
                        int8_t prob = in_ptr[(5 + k) * grid_len];
                        if (prob > maxClassProbs)
                        {
                            maxClassId = k;
                            maxClassProbs = prob;
                        }
                    }
                    if (maxClassProbs>thres_i8){
                        boxes.push_back(box_x);
                        boxes.push_back(box_y);
                        boxes.push_back(box_w);
                        boxes.push_back(box_h);
                        objProbs.push_back(sigmoid(deqnt_affine_to_f32(maxClassProbs, zp, scale))* sigmoid(deqnt_affine_to_f32(box_confidence, zp, scale)));
                        classId.push_back(maxClassId);
                        validCount++;
                    }
                }
            }
        }
    }
    return validCount;
}

// 输出变量、
int post_process(int8_t *input0, int8_t *input1, int8_t *input2, int model_in_h, int model_in_w,
                 float conf_threshold, float nms_threshold, float scale_w, float scale_h,
                 std::vector<int32_t> &qnt_zps, std::vector<float> &qnt_scales,
                 detect_result_group_t *group)
{

    RK_LOGD("==post_process config is model_in_h is %d, model_in_w is %d, threshold is %3f and %3f, scale_w is %4f, scale_h is %4f===\n",
            model_in_h, model_in_w, 
            conf_threshold, nms_threshold,
             scale_w, scale_h);
             
    RK_LOGD("===post_process==\n");

    static int init = -1;
    if (init == -1)
    {
        int ret = 0;
        ret = loadLabelName(LABEL_NALE_TXT_PATH, labels); //读取标签文件
        if (ret < 0)
        {
            return -1;
        }

        init = 0;
    }
    memset(group, 0, sizeof(detect_result_group_t));

    std::vector<float> filterBoxes;
    std::vector<float> objProbs;
    std::vector<int> classId;
    
    // stride 8
    // RK_LOGI("====stride is 8===\n");
    int stride0 = 8;
    int grid_h0 = model_in_h / stride0;
    int grid_w0 = model_in_w / stride0;
    // RK_LOGI("===grid_h0 is %d, grid_w0 is %d==\n", grid_h0, grid_w0);

    int validCount0 = 0;
    validCount0 = process(input0, (int *)anchor0, grid_h0, grid_w0, model_in_h, model_in_w,
                          stride0, filterBoxes, objProbs, classId, conf_threshold, qnt_zps[0], qnt_scales[0]);
    // RK_LOGI("===validCount0 is %d====\n",validCount0);

    // stride 16
    // RK_LOGI("====stride is 16===\n");
    int stride1 = 16;
    int grid_h1 = model_in_h / stride1;
    int grid_w1 = model_in_w / stride1;
    // RK_LOGI("===grid_h1 is %d, grid_w1 is %d==\n", grid_h1, grid_w1);
    int validCount1 = 0;
    validCount1 = process(input1, (int *)anchor1, grid_h1, grid_w1, model_in_h, model_in_w,
                          stride1, filterBoxes, objProbs, classId, conf_threshold, qnt_zps[1], qnt_scales[1]);
    // RK_LOGI("===validCount1 is %d===\n", validCount1);

    // stride 32
    // RK_LOGI("====stride is 32===\n");
    int stride2 = 32;
    int grid_h2 = model_in_h / stride2;
    int grid_w2 = model_in_w / stride2;
    // RK_LOGI("===grid_h2 is %d, grid_w2 is %d==\n", grid_h2, grid_w2);

    int validCount2 = 0;
    validCount2 = process(input2, (int *)anchor2, grid_h2, grid_w2, model_in_h, model_in_w,
                          stride2, filterBoxes, objProbs, classId, conf_threshold, qnt_zps[2], qnt_scales[2]);
    // RK_LOGI("===validCount2 is %d===\n", validCount2);

    int validCount = validCount0 + validCount1 + validCount2;
    // RK_LOGI("===validCount is %d===\n", validCount);
    // no object detect
    if (validCount <= 0)
    {
        return 0;
    }

    std::vector<int> indexArray;
    for (int i = 0; i < validCount; ++i)
    {
        indexArray.push_back(i);
    }

    quick_sort_indice_inverse(objProbs, 0, validCount - 1, indexArray);

    std::set<int> class_set(std::begin(classId),std::end(classId));
    
    for(auto c : class_set){
        nms(validCount, filterBoxes, classId, indexArray, c, nms_threshold);
    }

    int last_count = 0;
    group->count = 0;
    /* box valid detect target */
    // RK_LOGI("===vaildcount is %d===\n",validCount);
    for (int i = 0; i < validCount; ++i)
    {

        if (indexArray[i] == -1 || last_count >= OBJ_NUMB_MAX_SIZE)
        {
            continue;
        }
        int n = indexArray[i];

        float x1 = filterBoxes[n * 4 + 0];
        float y1 = filterBoxes[n * 4 + 1];
        float x2 = x1 + filterBoxes[n * 4 + 2];
        float y2 = y1 + filterBoxes[n * 4 + 3];
        int id = classId[n];
        float obj_conf = objProbs[i];

        // RK_LOGI("===x1 y1 x2 y2 is %f, %f, %f, %f==\n",x1, y1, x2, y2);
        // RK_LOGI("===probs is %f===\n", obj_conf);

        // group->results[last_count].box.left = (int)(clamp(x1, 0, model_in_w) / scale_w);
        // group->results[last_count].box.top = (int)(clamp(y1, 0, model_in_h) / scale_h);
        // group->results[last_count].box.right = (int)(clamp(x2, 0, model_in_w) / scale_w);
        // group->results[last_count].box.bottom = (int)(clamp(y2, 0, model_in_h) / scale_h);
        group->results[last_count].box.left = (int)x1;
        group->results[last_count].box.top = (int)y1;
        group->results[last_count].box.right = (int)x2;
        group->results[last_count].box.bottom = (int)y2;


        group->results[last_count].prop = obj_conf;
        char *label = labels[id];
        strncpy(group->results[last_count].name, label, OBJ_NAME_MAX_SIZE);

        // RK_LOGI("result %2d: (%4d, %4d, %4d, %4d), %s\n", i, group->results[last_count].box.left, group->results[last_count].box.top,
        //        group->results[last_count].box.right, group->results[last_count].box.bottom, label);
        last_count++;
    }
    group->count = last_count;

    return 0;
}


void InitLookupTable()
{
    int i;

	for (i = 0; i < 256; i++) RGBYUV14075[i] = (float)1.4075 * (i-128);
	for (i = 0; i < 256; i++) RGBYUV03455[i] = (float)0.3455 * (i - 128);
	for (i = 0; i < 256; i++) RGBYUV07169[i] = (float)0.7169 * (i - 128);
	for (i = 0; i < 256; i++) RGBYUV17790[i] = (float)1.7790 * (i - 128);

}

void InitLookupTablergb()
{
    int i;

	for (i = 0; i < 256; i++) RGBYUV02990[i] = (float)0.2990 * i;
	for (i = 0; i < 256; i++) RGBYUV05870[i] = (float)0.5870 * i;
	for (i = 0; i < 256; i++) RGBYUV01140[i] = (float)0.1140 * i;
	for (i = 0; i < 256; i++) RGBYUV01684[i] = (float)0.1684 * i;
	for (i = 0; i < 256; i++) RGBYUV03316[i] = (float)0.3316 * i;
	for (i = 0; i < 256; i++) RGBYUV04187[i] = (float)0.4187 * i;
	for (i = 0; i < 256; i++) RGBYUV00813[i] = (float)0.0813 * i;
}

int yuv2rgb(int x_dim, int y_dim, void* bmp, void* r_out, void* g_out, void* b_out, int flip)
{

	static int init_done = 0;

    long i, j, size;
	unsigned char* r, * g, * b;
	unsigned char* y, * u, * v;
	unsigned char* pu1, * pu2,* pv1, * pv2,* psu,* psv,* psy;
	unsigned char* y_buffer, *u_buffer, *v_buffer, *yuv_buffer;
	unsigned char *sub_u_buf, *sub_v_buf;
	float sub_r,sub_g,sub_b;
	
	if (init_done == 0)
	{
		InitLookupTable();
		init_done = 1;
	}

	if ((x_dim % 2) || (y_dim % 2)) return 1;
	size = x_dim * y_dim;

	
	y_buffer = (unsigned char*)malloc(size* sizeof(unsigned char));
	u_buffer = (unsigned char*)malloc(size* sizeof(unsigned char) / 4);
	v_buffer = (unsigned char*)malloc(size* sizeof(unsigned char) / 4);
	sub_u_buf = (unsigned char*)malloc(size* sizeof(unsigned char));
	sub_v_buf = (unsigned char*)malloc(size* sizeof(unsigned char));
	yuv_buffer = (unsigned char*)bmp;
	y = y_buffer;
	u = u_buffer;
	v = v_buffer;

	r = (unsigned char*)r_out;
	g = (unsigned char*)g_out;
	b = (unsigned char*)b_out;

	for (i = 0; i < size; i ++) 
	{
		*y = *(yuv_buffer + i);
		y++;
	}

	for (i = size; i < size * 5 / 4; i++) 
	{
		*u = *(yuv_buffer + i);
		u++;
	}
	for (i = size * 5 / 4; i < size * 3 / 2; i ++) 
	{
		*v = *(yuv_buffer + i);
		v++;
	}

	for (i = 0;  i < y_dim / 2; i ++) 
	{
		pu1 = sub_u_buf + i * 2 * x_dim;
		pu2 = sub_u_buf + (i * 2 + 1) * x_dim;
		pv1 = sub_v_buf + i * 2 * x_dim;
		pv2 = sub_v_buf + (i * 2 + 1) * x_dim;
		for (j = 0; j < x_dim / 2;j++) 
		{
			*pu1 = *(u_buffer + j + i * x_dim / 2);
			*pu2 = *pu1;
			pu1++;
			pu2++;
			*pu1 = *(u_buffer + j + i * x_dim / 2);
			*pu2 = *pu1;
			pu1++;
			pu2++;
			*pv1 = *(v_buffer + j + i * x_dim / 2);
			*pv2 = *pv1;
			pv1++;
			pv2++;
			*pv1 = *(v_buffer + j + i * x_dim / 2);
			*pv2 = *pv1;
			pv1++;
			pv2++;

		}
	}
	if (flip)
	{

		for (i = 0; i < size; i++)
		{
			sub_r = *(y_buffer+i) + RGBYUV14075[*(sub_v_buf+i)];
			sub_g = *(y_buffer+i) - RGBYUV03455[*(sub_u_buf+i)] - RGBYUV07169[*(sub_v_buf+i)];
			sub_b = *(y_buffer+i) + RGBYUV17790[*(sub_u_buf+i)];
		
			if (sub_r < 0) sub_r = 0;
			if (sub_r > 255) sub_r = 255;

			if (sub_g < 0) sub_g = 0;
			if (sub_g > 255) sub_g = 255;

			if (sub_b < 0) sub_b = 0;
			if (sub_b > 255) sub_b = 255;
		
			*r = (unsigned char)sub_r;
			*g = (unsigned char)sub_g;
			*b = (unsigned char)sub_b;
			r++;
			g++;
			b++;
		}
	}
	else {
		for (j = 0; j < y_dim; j++) 
		{
			psy = y_buffer + (y_dim - 1 - j) * x_dim;
			psu = sub_u_buf + (y_dim - 1 - j) * x_dim;
			psv = sub_v_buf+ (y_dim - 1 - j) * x_dim;
			for (i = 0; i < x_dim; i++) 
			{
				sub_r = *(psy+i) + RGBYUV14075[*(psv+i)];
				sub_g = *(psy+i) - RGBYUV03455[*(psu+i)]- RGBYUV07169[*(psv+i)];
				sub_b = *(psy+i) + RGBYUV17790[*(psu+i)];

				if (sub_r < 0) sub_r = 0;
				if (sub_r > 255) sub_r = 255;

				if (sub_g < 0) sub_g = 0;
				if (sub_g > 255) sub_g = 255;

				if (sub_b < 0) sub_b = 0;
				if (sub_b > 255) sub_b = 255;

				*r = (unsigned char)(sub_r);
				*g = (unsigned char)(sub_g);
				*b = (unsigned char)(sub_b);
				r++;
				g++;
				b++;

			}
		}
	
	}
	free(y_buffer);
	free(u_buffer);
	free(v_buffer);
	free(sub_u_buf);
	free(sub_v_buf);
	return 0;

}

int rgb2yuv(int x_dim, int y_dim, void *bmp, void *y_out, void *u_out, void *v_out, int flip)
{
    	static int init_done = 0;

	long i, j, size;
	unsigned char *r, *g, *b;
	unsigned char *y, *u, *v;
	unsigned char *pu1, *pu2, *pv1, *pv2, *psu, *psv;
	unsigned char *y_buffer, *u_buffer, *v_buffer;
	unsigned char *sub_u_buf, *sub_v_buf;

	if (init_done == 0)
	{
		InitLookupTablergb();
		init_done = 1;
	}

	
	if ((x_dim % 2) || (y_dim % 2)) return 1;
	size = x_dim * y_dim;

	
	y_buffer = (unsigned char *)y_out;
	sub_u_buf = (unsigned char *)u_out;
	sub_v_buf = (unsigned char *)v_out;
	u_buffer = (unsigned char *)malloc(size * sizeof(unsigned char));
	v_buffer = (unsigned char *)malloc(size * sizeof(unsigned char));
	if (!(u_buffer && v_buffer))
	{
		if (u_buffer) free(u_buffer);
		if (v_buffer) free(v_buffer);
		return 2;
	}

	b = (unsigned char *)bmp;
	y = y_buffer;
	u = u_buffer;
	v = v_buffer;

	if (!flip) {
		for (j = 0; j < y_dim; j ++)
		{
			y = y_buffer + (y_dim - j - 1) * x_dim;
			u = u_buffer + (y_dim - j - 1) * x_dim;
			v = v_buffer + (y_dim - j - 1) * x_dim;

			for (i = 0; i < x_dim; i ++) {
				g = b + 1;
				r = b + 2;
				*y = (unsigned char)(  RGBYUV02990[*r] + RGBYUV05870[*g] + RGBYUV01140[*b]);
				*u = (unsigned char)(- RGBYUV01684[*r] - RGBYUV03316[*g] + (*b)/2          + 128);
				*v = (unsigned char)(  (*r)/2          - RGBYUV04187[*g] - RGBYUV00813[*b] + 128);
				b += 3;
				y ++;
				u ++;
				v ++;
			}
		}
	} else {
		for (i = 0; i < size; i++)
		{
			g = b + 1;
			r = b + 2;
			*y = (unsigned char)(  RGBYUV02990[*r] + RGBYUV05870[*g] + RGBYUV01140[*b]);
			*u = (unsigned char)(- RGBYUV01684[*r] - RGBYUV03316[*g] + (*b)/2          + 128);
			*v = (unsigned char)(  (*r)/2          - RGBYUV04187[*g] - RGBYUV00813[*b] + 128);
			b += 3;
			y ++;
			u ++;
			v ++;
		}
	}


	for (j = 0; j < y_dim/2; j ++)
	{
		psu = sub_u_buf + j * x_dim / 2;
		psv = sub_v_buf + j * x_dim / 2;
		pu1 = u_buffer + 2 * j * x_dim;
		pu2 = u_buffer + (2 * j + 1) * x_dim;
		pv1 = v_buffer + 2 * j * x_dim;
		pv2 = v_buffer + (2 * j + 1) * x_dim;
		for (i = 0; i < x_dim/2; i ++)
		{
			*psu = (*pu1 + *(pu1+1) + *pu2 + *(pu2+1)) / 4;
			*psv = (*pv1 + *(pv1+1) + *pv2 + *(pv2+1)) / 4;
			psu ++;
			psv ++;
			pu1 += 2;
			pu2 += 2;
			pv1 += 2;
			pv2 += 2;
		}
	}

	free(u_buffer);
	free(v_buffer);

	return 0;

}